
#include "stdafx.h"
#include <math.h>
#include "S4ProjetDlg.h"
#include "craneexports.h"
#include "craneControl.h"

craneControl::craneControl()
{
}
void craneControl::craneControlInit(CS4ProjetDlg* Dlg)
{
	_parent = Dlg;
	_crane = new Crane();
	startSensorThread();
}
bool craneControl::startMoveThread()
{
	_moveThread = AfxBeginThread(MoveCrane,(void*)_parent,THREAD_PRIORITY_NORMAL,0,0);

	if(_moveThread)
	{
		return true;
	}
	else
	{
		return false;
	}
}
bool craneControl::startSensorThread()
{
	_sensorThread = AfxBeginThread(UpdateSensors,(void*)_parent,THREAD_PRIORITY_NORMAL,0,0);

	if(_sensorThread)
	{
		return true;
	}
	else
	{
		return false;
	}
}
UINT craneControl::UpdateSensors(LPVOID lParam)
{
	int		i,
			err = 1;
	float	sen[5];

	CS4ProjetDlg* Dlg = static_cast<CS4ProjetDlg*>(lParam);

	CStatic* CapXLabel		= (CStatic*)Dlg->GetDlgItem(ST_CAPX);
	CStatic* CapYLabel		= (CStatic*)Dlg->GetDlgItem(ST_CAPY);
	CStatic* CapRLabel		= (CStatic*)Dlg->GetDlgItem(ST_CAPR);
	CStatic* CapAlphaLabel	= (CStatic*)Dlg->GetDlgItem(ST_CAPALPHA);
	CStatic* CapBetaLabel	= (CStatic*)Dlg->GetDlgItem(ST_CAPBETA);
	CStatic* PosXLabel		= (CStatic*)Dlg->GetDlgItem(ST_POSX);
	CStatic* PosYLabel		= (CStatic*)Dlg->GetDlgItem(ST_POSY);
	CStatic* PosZLabel		= (CStatic*)Dlg->GetDlgItem(ST_POSZ);

	while(1)
	{
		Crane* crane = Dlg->CraneControl->_crane;

		err = crane->GetSensor(sen);

		if(err == ERR_NULL_PTR)
		{
			return 0;
		}
		else if(err == ERR_READ_DSP)
		{
			return 0;
		}

		else if(err == 0)
		{
			sen[0] = sen[0] + sen[2]*cos(sen[3])*sin(sen[4]);
			sen[1] = sen[1] + sen[2]*sin(sen[3]);
		 	sen[2] = sen[2]*cos(sen[3])*cos(sen[4]);
			
			char buff[10];
			sprintf(buff, "%d", sen[0]);

			//ADD SENSOR INFO TO ALL LABELS

			CapXLabel->SetWindowText((LPCTSTR)buff);
			CapYLabel->SetWindowText((LPCTSTR)buff);		
			CapRLabel->SetWindowText((LPCTSTR)buff);		
			CapAlphaLabel->SetWindowText((LPCTSTR)buff);	
			CapBetaLabel->SetWindowText((LPCTSTR)buff);
		}
		::Sleep(500);
	}

	return 0;
}


UINT craneControl::MoveCrane(LPVOID lParam)
{
	

	//CHECK WHETHER TRAJ/COMP/CAP FILES ARE LOADED
	CS4ProjetDlg* Dlg = static_cast<CS4ProjetDlg*>(lParam);
	Crane* crane = Dlg->CraneControl->_crane;

	crane->SetTrajMode(HOLD);

	crane->Run(CLOSE, RESTART);
	crane->Wait(STOP);

	//NEW TRAJECTORY FUNCTION GOES HERE
	//GenTraj(_crane,1.5,0.7,0.4);

	//Executer le deplacement une seconde fois
	crane->Run(CLOSE, RESTART);
	crane->Wait(STOP);

	//Fermer le fichier des capteurs
	crane->CloseSensorFile();
	
	return 0;
}